from pluginsCategories import pySKFWServices
import os,sys,logging,time,threading
from pykondo import *

class pySKFWMotion(pySKFWServices):
    name = 'pySKFWMotion'
    ki = None
    maxServos = 17
    servoPos =[]
    motionsList = {
        0:"Hello_KHR3(V1.1)",
        1:"Greeting",
        2:"Home_Position(Hello_KHR3)",
        3:"Wave",
        4:"Hip,_hipm_hurray!_Pull",
        5:"Chagrined_Drop",
        6:"Headstand",
        7:"Clap",
        8:"Clap_M40_10__times",
        9:"3-3-7_rhythm_clap",
        10:"Push-ups",
        11:"One_legged_knee__bend",
        12:"Bunny_hop_A",
        13:"Bunny_hop_B",
        14:"Stand_up_stomach",
        15:"Stand_up_back",
        16:"Safe_walk_(forward_counter_5_steps)",
        17:"Safe_walk_(backward_counter_5_steps)",
        18:"Safe_walk_(left_step_counter_5_steps)",
        19:"Safe_walk_(right_step_counter_5_steps)",
        20:"Quick_turn_A_left",
        21:"Quick_turn_A_right",
        22:"Regular_walk_5",
        23:"Regular_walk_backward",
        24:"Regular_walk_left",
        25:"Regular_walk_right",
        26:"Kick_ball_forward_left",
        27:"Kick_ball_forward_right",
        28:"Kick_ball_sideways_left",
        29:"Kick_ball_sideways_right",
        30:"Kick_ball_backward_left",
        31:"Kick_ball_backward_right"
    }
    def init(self,server):
        self.server = server
        self.provides = {
            'getMotionsList'    : {'handler':self.getMotionList    ,'resultType':'lst'  ,'path':r"/ki/listmotions"},
            'initKondo'         : {'handler':self.initKondo        ,'resultType':'lst'  ,'path':r"/ki/initKondo"},
            'knd_run_motion'    : {'handler':self.knd_run_motion   ,'resultType':'lst'  ,'path':r"/ki/runmotion/([0-9]+)"},
            'knd_stop_motion'   : {'handler':self.knd_stop_motion  ,'resultType':'lst'  ,'path':r"/ki/stopmotion"},
            'knd_read_analog'   : {'handler':self.knd_read_analog  ,'resultType':'lst'  ,'path':r"/ki/knd_read_analog"},
            'knd_status'        : {'handler':self.knd_status       ,'resultType':'lst'  ,'path':r"/ki/status"},
            'knd_getServoPos'   : {'handler':self.knd_getServoPos  ,'resultType':'lst'  ,'path':r"/ki/getservopositions"}
        }
        #self.server.initTimer(self.name,'servosPos',0.5,self.tickDatas)
        #logging.info("serving Motions")
        #logging.info("serving Motions ok")
        self.servoPosSemaphore = threading.BoundedSemaphore(1)		
    def start(self):
        pass
    def stop(self):
        if (self.ki!=None and self.ki>0):
            kondo_close(self.ki)

    def getMotionList(self):
        return self.motionsList
        
    def initKondo(self):
        if (self.ki==None or self.ki<0):
            ki = KondoInstance()
            try:
                ret = kondo_init(ki)
                if ret < 0:
                    sys.exit(ki.error)
                    return -1
            except:
                logging.info("no kondo "+ki.error)
                return -1
        logging.info("Kondo OK")
        self.ki=ki
        return self.ki
        
    def knd_run_motion(self,motionId):
        self.initKondo()
        max_wait = 50 ;
        motion = int(motionId)-1 
        rtn={}
        rtn['motion']=motion
        rtn['time']=str(time.localtime()[7])
        if self.ki:
            ret = kondo_stop_motion(self.ki)
            ret = kondo_play_motion(self.ki,motion , max_wait)
        return rtn

    def knd_stop_motion(self):
        self.initKondo()
        ret=-1
        if self.ki:
            try:
                ret = kondo_stop_motion(self.ki)
            except:
                print "rrrr"
                print E
            logging.info("stop motion %s" % (ret))
            
        return ret
    
    def knd_status(self):
        rtn = {}
        rtn['battery_level']=self.knd_read_analog(0)
        rtn['connected']=(rtn['battery_level']!=64)
        return rtn

    def knd_read_analog(self,port):
        self.initKondo()
        if self.ki:
            foo=0
            ret = kondo_read_analog(self.ki,foo, port)
            if ret < 0:
                return -1
        return ret[1]
        
    def knd_getServoPos(self):
        self.servoPosSemaphore.acquire()
        arrPos = self.arrPositions
        self.servoPosSemaphore.release()
        return arrPos

    def tickDatas(self):   
        #return True
        self.servoPosSemaphore.acquire()
        self.initKondo()
        self.arrPositions = {'servos':[],'AD':{}}
        if self.ki:
            for i in range(1,self.maxServos+1):
                self.arrPositions['servos'].append({
                'index'	: i,
                    'pos' 	: kondo_get_servo_pos(self.ki,i),
                    'trim'	: kondo_get_servo_pos(self.ki,i)
                })
            self.arrPositions['AD'][0]=self.knd_read_analog(0)
            self.arrPositions['AD'][1]=self.knd_read_analog(1)	
        self.servoPosSemaphore.release()
        
